#include "node_ll.h"
#include "common_tool/angle_helper.h"
#include "perpend_point2d_helper.h"
#include <cmath>
#include <limits>
#include <climits>
#include "common_tool/geometry.hpp"
#include "common_tool/csv_writer.h"
#include <memory>
#include <chrono>
#include "node_config_path.h"

namespace auto_ros
{
namespace control
{
NodeLL::NodeLL(
	double loop_rate, std::string node_yaml_file_name)
	: NodeBase(loop_rate, node_yaml_file_name),
	  speed_controller_(0.2, 0.001, 0, 1 / loop_rate, -0.2, 0.2, -0.2, 0.2)

{

	// filter parameters
	const float samplingrate = 50;		 // Hz
	const float cutoff_frequency = 10.0; // Hz
	// calc the coefficients
	low_pass_f_.setup(samplingrate, cutoff_frequency);
	std::string vehicle_lmpc_yaml_file = "vehicle_lmpc.yaml";
	VehicleLMpcFactory lmpc_factory;
	mpc_ptr_ = lmpc_factory.CreateSharedObject("VehicleLLMpc",
											   node_config_path + "VehicleLLMpc/", vehicle_lmpc_yaml_file, 50);
	if (mpc_ptr_ == nullptr)
	{
		std::cout << "\033[31m Error:NodeLL::NodeLL failed,can not create mpc_ptr_ \033[0m" << std::endl;
		throw 100;
	}

	unsigned int state_dim = mpc_ptr_->vehicle_dyn_ptr_->state_dim();
	unsigned int control_dim = mpc_ptr_->vehicle_dyn_ptr_->control_dim();
	unsigned int distur_dim = mpc_ptr_->vehicle_dyn_ptr_->distur_dim();
	//init mpc_ptr_ update_imput	Eigen::MatrixXd q_;
	x0_ = Eigen::VectorXd::Zero(state_dim);
	u_fb_ = Eigen::VectorXd::Zero(control_dim);

	//std::cout << mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_ << std::endl;
}

void NodeLL::HardBrake()
{
	control_cmd_.brake = 0;
	control_cmd_.throttle = 0;
	speed_controller_.resetIntergration();
}
void NodeLL::HardBrake(double brake_pre)
{
	control_cmd_.brake = brake_pre;
	control_cmd_.throttle = 0;
	speed_controller_.resetIntergration();
}
bool NodeLL::UpdateInput()
{
	double temp_angle = auto_ros::common_tool::angle_mins_pi2pi(ref_point_interp_.theta) + 0.5 * M_PI;
	double heading_error = auto_ros::common_tool::angle_mins_pi2pi(localizition_msg_.heading - ref_point_interp_.theta);
	double planning_speed;
	if (node_yaml_["is_overwrite_speed"].as<int>() == 1)
	{
		planning_speed = node_yaml_["test_speed"].as<double>();
	}
	else
	{
		planning_speed = ref_point_interp_.v;
	}

	x0_ << localizition_msg_.vx - planning_speed,
		localizition_msg_.vy,
		localizition_msg_.avz / 180 * M_PI,
		-lateral_error_, localizition_msg_.ve * std::cos(temp_angle) + localizition_msg_.vn * std::sin(temp_angle); //localizition_msg_.ve * std::cos(temp_angle) + localizition_msg_.vn * std::sin(temp_angle);

	double tire_f_angle = chassis_msg_.steering_angle_f / 180 * 3.14;
	if (tire_f_angle > mpc_ptr_->mpc_yaml_["max_tire_f_angle"].as<double>())
	{
		tire_f_angle = mpc_ptr_->mpc_yaml_["max_tire_f_angle"].as<double>();
	}
	if (tire_f_angle < -mpc_ptr_->mpc_yaml_["max_tire_f_angle"].as<double>())
	{
		tire_f_angle = -mpc_ptr_->mpc_yaml_["max_tire_f_angle"].as<double>();
	}
	//calc acc
	double temp_acc = chassis_msg_.torque_f * mpc_ptr_->mpc_yaml_["gear_ratio"].as<double>() /
					  mpc_ptr_->mpc_yaml_["tire_raduis"].as<double>() /
					  mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["mass"].as<double>();
	temp_acc -= chassis_msg_.mc_pressure * mpc_ptr_->mpc_yaml_["brake_nm_per_Mpa"].as<double>() /
				mpc_ptr_->mpc_yaml_["tire_raduis"].as<double>() /
				mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["mass"].as<double>();
	u_fb_ << tire_f_angle, temp_acc; //last_steer_angle_ / 180 * M_PI; //
	speed_ = chassis_msg_.speed_mps;
	kappa_ = ref_point_interp_.kappa;
	ref_point_.x_ = ref_point_interp_.x;
	ref_point_.y_ = ref_point_interp_.y;
	ref_point_.theta_ = ref_point_interp_.theta;
	ref_point_.kappa_ = ref_point_interp_.kappa;
	ref_point_.v_ = ref_point_interp_.v;
	ref_point_.s_ = ref_point_interp_.s;
	if (node_yaml_["is_overwrite_speed"].as<int>() == 1)
	{
		ref_point_.a_ = 0.0;
	}
	else
	{
		ref_point_.a_ = ref_point_interp_.a;
	}

	Eigen::VectorXd state_esti = mpc_ptr_->kf_lateral_ptr_->state_esti();
	//Eigen::VectorXd state_esti = mpc_ptr_->kf_lateral_ptr_->state_esti();
	if (node_yaml_["is_log"].as<int>() == 1 && start_log_ == true)
	{
		double temp_speed = 1.0;
		if (std::abs(chassis_msg_.speed_mps) < 1.0)
		{
			if (chassis_msg_.speed_mps < 0)
				temp_speed = -1.0;
			else
				temp_speed = 1.0;
		}
		else
		{
			temp_speed = chassis_msg_.speed_mps;
		}

		static std::shared_ptr<io::CSVWriter> log_writer = std::make_shared<io::CSVWriter>(log_file_name_);
		//static io::CSVWriter x0_writer("x0_writer.csv");
		static std::vector<std::string> csv_header =
			{
				"vx",
				"vx_ref",
				"vy",
				"vy_ref",
				"yaw_rate",
				"yaw_rate_ref",
				"e_y",
				"e_y_ref",
				"v_gama",
				"v_gama_ref",
				"heading_error",
				"kappa",
				"tire_f_angle",
				"ref_lambda_",
				"planning_theta",
				"x",
				"y",
				"y_ref",
				"alpha_f",
				"alpha_r",
				"f_angle_ref",
				"s",
				"speed",
				"vx_esti",
				"vy_esti",
				"yaw_rate_esti",
				"ax_dis_esti",
				"ay_dis_esti",
				"yaw_acc_dis_esti",
				"ax",
				"ax_ref", "time_to_solve"};
		static bool temp_bool = false;
		if (temp_bool == false)
		{
			log_writer->write_row<std::string>(csv_header);
			temp_bool = true;
		}
		std::vector<double> temp_vector =
			{localizition_msg_.vx, ref_point_interp_.v, x0_[1], mpc_ptr_->x_ref_(1),
			 x0_[2], mpc_ptr_->x_ref_(2), x0_[3], mpc_ptr_->x_ref_(3), x0_[4], mpc_ptr_->x_ref_(4),
			 auto_ros::common_tool::angle_mins_pi2pi(localizition_msg_.heading - ref_point_interp_.theta),
			 ref_point_interp_.kappa,
			 tire_f_angle, ref_lambda_,
			 ref_point_interp_.theta,
			 localizition_msg_.x,
			 localizition_msg_.y,
			 ref_lambda_ * ref_point_closet_.y + (1 - ref_lambda_) * ref_point_second_closet_.y,
			 180 / 3.14 * (x0_[1] / temp_speed + mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>() * x0_[2] / temp_speed - tire_f_angle),
			 180 / 3.14 * (x0_[1] / temp_speed - mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>() * x0_[2] / temp_speed),
			 mpc_ptr_->u_ref_(0), ref_point_interp_.s, temp_speed,
			 state_esti(0), state_esti(1), state_esti(2), state_esti(3),
			 state_esti(4), state_esti(5), mpc_ptr_->vehicle_mpc_ptr_->get_contorl_one_shot(0)(1), mpc_ptr_->u_ref_(1),
			 debug_msg_.time_to_solve};
		log_writer->write_row<double>(temp_vector);
	}
	return true;
}
void NodeLL::UpdateDebugMsg()
{

	//debug_msg_.tire_f_angle_max_ = tire_f_angle_max_;
	kappa_ = ref_point_interp_.kappa;
	Eigen::VectorXd state_esti = mpc_ptr_->kf_lateral_ptr_->state_esti();
	debug_msg_.error_speed = x0_(0);
	debug_msg_.vy_ref = mpc_ptr_->x_ref_(1);
	debug_msg_.yaw_rate_ref = mpc_ptr_->x_ref_(2);
	debug_msg_.lateral_error_ref = mpc_ptr_->x_ref_(3);
	debug_msg_.speed_gama_ref = mpc_ptr_->x_ref_(4);

	debug_msg_.vy_esti = state_esti(1);
	debug_msg_.yaw_rate_esti = state_esti(2);
	debug_msg_.acc_x_distur = state_esti(3);
	debug_msg_.acc_y_distur = state_esti(4);
	debug_msg_.acc_yaw_distur = state_esti(5);
	debug_msg_.lateral_speed = x0_(1);
	debug_msg_.yaw_rate = x0_(2);
	debug_msg_.lateral_error = x0_(3);
	debug_msg_.speed_gama = x0_(4);
	debug_msg_.kappa = ref_point_interp_.kappa;
	debug_msg_.lambda = ref_lambda_;
	debug_msg_.tire_f_angle_cmd = u_fb_(0) * 180 / 3.14;
	debug_msg_.theta = auto_ros::common_tool::angle_mins_180to180(
		ref_point_closet_.theta);

	double temp_speed = 1.0;
	if (std::abs(chassis_msg_.speed_mps) < 1.0)
	{
		if (chassis_msg_.speed_mps < 0)
			temp_speed = -1.0;
		else
			temp_speed = 1.0;
	}
	else
	{
		temp_speed = chassis_msg_.speed_mps;
	}
	debug_msg_.f_alpha = (localizition_msg_.vy + mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>() * x0_(2)) / temp_speed - u_fb_(0);
	debug_msg_.r_alpha = (localizition_msg_.vy - mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>() * debug_msg_.yaw_rate) / temp_speed;
	double temp_f_n_flapha = debug_msg_.f_alpha /
							 (mpc_ptr_->mpc_yaml_["alpha_f_max"].as<double>() / 180 * M_PI);
	double temp_f_a = u_fb_(1) / mpc_ptr_->mpc_yaml_["acc_f_x_max"].as<double>();
	debug_msg_.tire_utilization = std::pow(std::pow(temp_f_n_flapha, 2) + std::pow(temp_f_a, 2), 0.5);
	debug_msg_.vy_ref = mpc_ptr_->x_ref_(1);
	debug_msg_.yaw_rate_ref = mpc_ptr_->x_ref_(2);
	debug_msg_.lateral_error_ref = mpc_ptr_->x_ref_(3);
	debug_msg_.speed_gama_ref = mpc_ptr_->x_ref_(4);
	debug_msg_.f_angle_ref = mpc_ptr_->u_ref_(0) * 180 / 3.14;
	debug_msg_.heading_error = auto_ros::common_tool::angle_mins_pi2pi(localizition_msg_.heading - ref_point_interp_.theta);
}
bool NodeLL::LateralCtr()
{
	auto start = std::chrono::high_resolution_clock::now();

	if (!mpc_ptr_->step(x0_, u_fb_, ref_point_, speed_))
	{
		std::cout << "\033[31m NodeLL::LateralCtr failed, \033[0m" << std::endl;
		return false;
	}
	auto end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double, std::milli> tm = end - start;
	debug_msg_.time_to_solve = tm.count();

	std::cout << "##########################" << tm.count() << std::endl;									// 毫秒
	control_cmd_.tire_angle_target_f = mpc_ptr_->vehicle_mpc_ptr_->get_contorl_one_shot(0)(0) * 180 / 3.14; //low_pass_f_.filter(mpc_ptr_->vehicle_mpc_ptr_->get_contorl_one_shot(0)(0) * 180 / 3.14); //;vehicle_mpc_ptr_->get_optimal_ctr()(0, 0) / rate_ * 180 / 3.14;
	control_cmd_.tire_angle_target_r = 0;
	double acc_cmd = mpc_ptr_->vehicle_mpc_ptr_->get_contorl_one_shot(0)(1);
	if (acc_cmd >= 0)
	{
		control_cmd_.torque_f = acc_cmd * mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["mass"].as<double>() *
								mpc_ptr_->mpc_yaml_["tire_raduis"].as<double>() /
								mpc_ptr_->mpc_yaml_["gear_ratio"].as<double>();
		control_cmd_.brake = 0; //Mpa
	}
	else
	{
		control_cmd_.torque_f = 0;
		control_cmd_.brake = std::abs(acc_cmd) * mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["mass"].as<double>() *
							 mpc_ptr_->mpc_yaml_["tire_raduis"].as<double>() /
							 mpc_ptr_->mpc_yaml_["brake_nm_per_Mpa"].as<double>();
	}

	last_steer_angle_ = control_cmd_.tire_angle_target_f; //vehicle_mpc_ptr_->get_optimal_ctr()(1, 0) / rate_ * 180 / 3.14;
	return true;
}
bool NodeLL::LongtitudeCtr()
{
}
void NodeLL::LoopProc()
{

	UpdateDebugMsg();

	debug_pub_.publish(debug_msg_);
	debug_msg_.is_solved = 0.0;

	/*if (chassis_msg_.driving_mode != 1)
	{
		std::cout << "\033[31m NodeLL::LoopProc  failed,chassis_msg_.driving_mode != 1(not auto control) \033[0m" << std::endl;
		mpc_ptr_->reset_kf();
		return;
	}*/
	NoFood();
	if (!CheckDog())
	{
		std::cout << "\033[31m NodeLL::LoopProc-CheckDog  failed, \033[0m" << std::endl;
		HardBrake();
		mpc_ptr_->reset_kf();
		control_cmd_pub_.publish(control_cmd_);
		return;
	}

	if (!SearchRefPoint(planning_msg_))
	{
		std::cout << "\033[31m NodeLL::LoopProc-SearchRefPoint  failed, \033[0m" << std::endl;
		HardBrake();
		control_cmd_pub_.publish(control_cmd_);
		mpc_ptr_->reset_kf();
		return;
	}
	//UpdateDebugMsg();
	if (!UpdateInput())
	{
		std::cout << "\033[31m NodeLL::LoopProc-UpdateInput  failed, \033[0m" << std::endl;
		HardBrake();
		control_cmd_pub_.publish(control_cmd_);
		mpc_ptr_->reset_kf();
		return;
	}

	/*if (auto_msg_dog_.isDied() || auto_ctr_msg_.ctr_mode <= 1)
	{
		start_log_ = false;
		std::cout << "\033[31m NodeLL::auto_msg_dog_ timeout  failed, \033[0m" << std::endl;
		HardBrake(0.3);
		control_cmd_pub_.publish(control_cmd_);
		mpc_ptr_->reset_kf();
		return;
	}
	else
	{
		start_log_ = true;
	}*/
	start_log_ = true;
	if (!LateralCtr())
	{
		std::cout << "\033[31m NodeLL::LoopProc-LateralCtr  failed, \033[0m" << std::endl;
		mpc_ptr_->reset_kf();
		HardBrake();
		//control_cmd_pub_.publish(control_cmd_);
		return;
	}
	LongtitudeCtr();
	control_cmd_pub_.publish(control_cmd_);
	debug_msg_.is_solved = 1.0;
}
} // namespace control
} // namespace auto_ros